#include <Arduino.h>
#include "sg90.h"

uint8_t pwm_list[4] = {PWM_RECYCLE_PIN, PWM_KITCHEN_PIN, PWM_HARMFULE_PIN, PWM_OTHER_PIN };
uint8_t pwm_channel[4] = { 8, 9 ,10, 11};
void sg90_init(void)
{
    int channel = 8;    // 通道(高速通道（0 ~ 7）由80MHz时钟驱动，低速通道（8 ~ 15）由 1MHz 时钟驱动。)
    int freq = 50;      // 频率(20ms周期)
    int resolution = 8; // 分辨率

    for(int i = 0; i < 4; i++)
    {
        ledcSetup(channel, freq, resolution); // 设置通道
        ledcAttachPin(pwm_list[i], channel);          // 将通道与对应的引脚连接
        channel +=1;
    }
}

int calculatePWM(int degree)
{ //0-180度
 //20ms周期，高电平0.5-2.5ms，对应0-180度角度
  const float deadZone = 6.4;//对应0.5ms（0.5ms/(20ms/256）) 舵机转动角度与占空比的关系：(角度/90+0.5)*255/20
  const float max = 32;//对应2.5ms
  if (degree < 0)
    degree = 0;
  if (degree > 180)
    degree = 180;
  return (int)(((max - deadZone) / 180) * degree + deadZone);
}

/**
 * @brief 更改垃圾桶盖子开关状态
 * @param garbage_num uint8_t 需要操作的垃圾桶序号
 * @param stat bool STATE_OPEN 开盖   STATE_CLOSE 关盖
 * @return int 0：成功  else:失败
 */
int sg90_change_lid_stat(uint8_t garbage_num, bool stat)
{
    if ((garbage_num < 0) || (garbage_num > 3))
    {
        return -1;
    }
    else{
        if(stat == STATE_OPEN)
        {
            ledcWrite(pwm_channel[garbage_num], calculatePWM(90));
        }
        else
        {
            ledcWrite(pwm_channel[garbage_num], calculatePWM(0));
        }
        return 0;
    }  
}